// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MicoKinematicConfigurations.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Mico
{
    using System;
    using System.Collections.Generic;
    using System.IO;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// Kinematic configuration of Mico arm
    /// </summary>
    public class MicoKinematicConfigurations
    {
        /// <summary>
        /// Create the 6DOF kinematic chain of Mico robot arm
        /// Detailed documentation can be found online in its website: $http://kinovarobotics.com/products/mico-research-edition$
        /// </summary>
        /// <param name="endEffectorOffsetInLastJoint">Offset transformation of the end effector in last joint's coordinate frame, default is zero offset</param>
        /// <returns>A kinematic chain</returns>
        public static KinematicChain CreateMicoKinematicChain(Matrix4 endEffectorOffsetInLastJoint = null)
        {
            int numJoints = 6;

            // some constants from the document specifying the critical dimensions
            const double D1 = 0.2755;
            const double D2 = 0.2900;
            const double D3 = 0.1233;
            const double D4 = 0.0741;
            const double D5 = 0.0741;
            const double D6 = 0.1600;
            const double E2 = 0.0070;

            double aa = 30.0 * Math.PI / 180.0;
            double ca = Math.Cos(aa);
            double sa = Math.Sin(aa);
            double c2a = Math.Cos(2 * aa);
            double s2a = Math.Sin(2 * aa);
            double d4b = D3 + sa / s2a * D4;
            double d5b = sa / s2a * D4 + sa / s2a * D5;
            double d6b = sa / s2a * D5 + D6;

            // create the joint list
            const double TenKDegreesInRadians = 10000 * MathConstants.Degrees2Radians;
            Joint[] jointList = new Joint[numJoints];
            jointList[0] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(Math.PI, 0, -D1), 0, -TenKDegreesInRadians, TenKDegreesInRadians);
            jointList[1] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 0), -MathConstants.PIOverTwo, 50 * MathConstants.Degrees2Radians, 310 * MathConstants.Degrees2Radians);
            jointList[2] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(Math.PI, D2, -E2), -MathConstants.PIOverTwo, 35 * MathConstants.Degrees2Radians, 325 * MathConstants.Degrees2Radians);
            jointList[3] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, -d4b), Math.PI, -TenKDegreesInRadians, TenKDegreesInRadians);
            jointList[4] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(2 * aa, 0, -d5b), 0, -TenKDegreesInRadians, TenKDegreesInRadians);
            jointList[5] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-2 * aa, 0, -d6b), -Math.PI, -TenKDegreesInRadians, TenKDegreesInRadians);

            // create the collision model for each link as well as the end effector
            double sphereRadius = 0.05;

            // link0 which is controlled by joint0
            List<GeometryElement3D> link0Elements = new List<GeometryElement3D>();
            SphereElement l0s0 = new SphereElement(new Vector3(0, 0, 0.1), 0.05);
            SphereElement l0s1 = new SphereElement(new Vector3(0, 0, 0.06), 0.05);
            SphereElement l0s2 = new SphereElement(new Vector3(0, 0, 0.02), 0.05);
            SphereElement l0s3 = new SphereElement(new Vector3(0, 0, -0.02), 0.055);
            link0Elements.Add(l0s0);
            link0Elements.Add(l0s1);
            link0Elements.Add(l0s2);
            GeometryElementGroup3D link0Group = new GeometryElementGroup3D(link0Elements);

            // link1 which is controlled by joint1
            List<GeometryElement3D> link1Elements = new List<GeometryElement3D>();
            SphereElement l1s0 = new SphereElement(new Vector3(0, 0, 0), 0.05);
            SphereElement l1s1 = new SphereElement(new Vector3(0.02, 0, 0), 0.05);
            SphereElement l1s2 = new SphereElement(new Vector3(0.06, 0, 0), 0.05);
            SphereElement l1s3 = new SphereElement(new Vector3(0.1, 0, 0), 0.05);
            SphereElement l1s4 = new SphereElement(new Vector3(0.14, 0, 0), 0.05);
            SphereElement l1s5 = new SphereElement(new Vector3(0.18, 0, 0), 0.05);
            SphereElement l1s6 = new SphereElement(new Vector3(0.22, 0, -0.005), 0.05);
            SphereElement l1s7 = new SphereElement(new Vector3(0.26, 0, 0), 0.055);
            SphereElement l1s8 = new SphereElement(new Vector3(0.3, 0, 0), 0.055);
            link1Elements.Add(l1s0);
            link1Elements.Add(l1s1);
            link1Elements.Add(l1s2);
            link1Elements.Add(l1s3);
            link1Elements.Add(l1s4);
            link1Elements.Add(l1s5);
            link1Elements.Add(l1s6);
            link1Elements.Add(l1s7);
            link1Elements.Add(l1s8);
            GeometryElementGroup3D link1Group = new GeometryElementGroup3D(link1Elements);

            // link2 which is controlled by joint2
            List<GeometryElement3D> link2Elements = new List<GeometryElement3D>();
            SphereElement l2s0 = new SphereElement(new Vector3(0, 0, 0.01), sphereRadius);
            SphereElement l2s1 = new SphereElement(new Vector3(0, -0.04, 0), sphereRadius);
            SphereElement l2s2 = new SphereElement(new Vector3(0, -0.08, -0.01), 0.04);
            SphereElement l2s3 = new SphereElement(new Vector3(0, -0.12, 0), 0.04);
            link2Elements.Add(l2s0);
            link2Elements.Add(l2s1);
            link2Elements.Add(l2s2);
            link2Elements.Add(l2s3);
            GeometryElementGroup3D link2Group = new GeometryElementGroup3D(link2Elements);

            // link3 which is controlled by joint3
            List<GeometryElement3D> link3Elements = new List<GeometryElement3D>();
            SphereElement l3s0 = new SphereElement(new Vector3(0, 0, 0), 0.038);
            SphereElement l3s1 = new SphereElement(new Vector3(0, 0.04, -0.02), 0.038);
            link3Elements.Add(l3s0);
            link3Elements.Add(l3s1);
            GeometryElementGroup3D link3Group = new GeometryElementGroup3D(link3Elements);

            // link4 which is controlled by joint4
            List<GeometryElement3D> link4Elements = new List<GeometryElement3D>();
            SphereElement l4s0 = new SphereElement(new Vector3(0, 0, 0), 0.038);
            SphereElement l4s1 = new SphereElement(new Vector3(0, 0, 0.04), 0.038);
            link4Elements.Add(l4s0);
            link4Elements.Add(l4s1);
            GeometryElementGroup3D link4Group = new GeometryElementGroup3D(link4Elements);

            // link5 which is controlled by joint5
            List<GeometryElement3D> link5Elements = new List<GeometryElement3D>();
            SphereElement l5s0 = new SphereElement(new Vector3(0, 0, 0.155), 0.037);
            link5Elements.Add(l5s0);
            GeometryElementGroup3D link5Group = new GeometryElementGroup3D(link5Elements);

            // putting all together
            // link6 does not have a collision model associated since it is a very small link
            // and is covered by the collision model of link5
            List<GeometryElementGroup3D> armCollisionModel = new List<GeometryElementGroup3D>();
            armCollisionModel.Add(link0Group);
            armCollisionModel.Add(link1Group);
            armCollisionModel.Add(link2Group);
            armCollisionModel.Add(link3Group);
            armCollisionModel.Add(link4Group);
            armCollisionModel.Add(link5Group);

            // the collision model of the end effector
            List<GeometryElement3D> endEffectorCollisionModel = new List<GeometryElement3D>();

            // palm
            SphereElement palm0 = new SphereElement(new Vector3(-0.022, 0, -0.11), 0.03);
            SphereElement palm1 = new SphereElement(new Vector3(0.022, 0, -0.11), 0.03);
            SphereElement palm2 = new SphereElement(new Vector3(-0.03, 0, -0.07), 0.03);
            SphereElement palm3 = new SphereElement(new Vector3(0.03, 0, -0.07), 0.03);
            SphereElement palm4 = new SphereElement(new Vector3(-0.05, 0, -0.03), 0.015);
            SphereElement palm5 = new SphereElement(new Vector3(0.05, 0, -0.03), 0.015);
            SphereElement palm6 = new SphereElement(new Vector3(-0.065, 0, -0.01), 0.015);
            SphereElement palm7 = new SphereElement(new Vector3(0.065, 0, -0.01), 0.015);
            SphereElement palm8 = new SphereElement(new Vector3(-0.08, 0, 0.01), 0.015);
            SphereElement palm9 = new SphereElement(new Vector3(0.08, 0, 0.01), 0.015);

            endEffectorCollisionModel.Add(palm0);
            endEffectorCollisionModel.Add(palm1);
            endEffectorCollisionModel.Add(palm2);
            endEffectorCollisionModel.Add(palm3);
            endEffectorCollisionModel.Add(palm4);
            endEffectorCollisionModel.Add(palm5);
            endEffectorCollisionModel.Add(palm6);
            endEffectorCollisionModel.Add(palm7);
            endEffectorCollisionModel.Add(palm8);
            endEffectorCollisionModel.Add(palm9);

            GeometryElementGroup3D handCollisionModel = new GeometryElementGroup3D(endEffectorCollisionModel);

            // arm geometry models
            string[] linkNameList = new string[7];
            linkNameList[0] = "base";
            linkNameList[1] = "link0";
            linkNameList[2] = "link1";
            linkNameList[3] = "link2";
            linkNameList[4] = "link3";
            linkNameList[5] = "link4";
            linkNameList[6] = "link5";

            string micoGeometryPath = @"Data\RobotModel\Mico\";
            string[] linkGeometryPaths = new string[7];
            linkGeometryPaths[0] = Path.Combine(micoGeometryPath, "base.off");
            linkGeometryPaths[1] = Path.Combine(micoGeometryPath, "link0.off");
            linkGeometryPaths[2] = Path.Combine(micoGeometryPath, "link1.off");
            linkGeometryPaths[3] = Path.Combine(micoGeometryPath, "link2.off");
            linkGeometryPaths[4] = Path.Combine(micoGeometryPath, "link3.off");
            linkGeometryPaths[5] = Path.Combine(micoGeometryPath, "link4.off");
            linkGeometryPaths[6] = Path.Combine(micoGeometryPath, "link5.off");
            
            string endEffectorName = "MicoGripper";
            string endEffectorGeometryPath = Path.Combine(micoGeometryPath, "micogripper.off");

            // create a kinematic chain
            KinematicChain kc = new KinematicChain(jointList, linkNameList, linkGeometryPaths, endEffectorName, endEffectorGeometryPath, armCollisionModel, handCollisionModel);
            
            // where the EE is w.r.t. the last joint
            Matrix4 endEffectorInLastJoint = (null == endEffectorOffsetInLastJoint) ? new Matrix4(new double[,] { { 0, 1, 0, 0 }, { 1, 0, 0, 0 }, { 0, 0, -1, 0 }, { 0, 0, 0, 1 } }) : endEffectorOffsetInLastJoint;
            kc.SetEndEffectorOffsetInLastJoint(endEffectorInLastJoint);

            return kc;
        }

        /// <summary>
        /// A function that generates equivalent joint configurations based on the current joint configuration.
        /// End effector pose remains the same for all these equivalent joint configurations.
        /// This function is absolutely hardware kinematic specific.  Some arms may not have an
        /// equivalent generator like this.
        /// </summary>
        /// <param name="jointValues">Current joint configuration</param>
        /// <param name="kc">Kinematic chain</param>
        /// <returns>Other joint values which are equivalent to the current one</returns>
        public static List<DoubleArray> MicoEquivalentJointConfigurationGenerator(DoubleArray jointValues, KinematicChain kc)
        {
            List<DoubleArray> pool = new List<DoubleArray>();

            pool.Add(jointValues);

            // let's loop through each joint individually and generate equivalent solution
            // by changing each of them
            for (int i = 0; i < jointValues.Count; ++i)
            {
                int numSolutionsSoFar = pool.Count;

                // only work on all the solutions we generated so far
                for (int j = 0; j < numSolutionsSoFar; ++j)
                {
                    DoubleArray newSolution = DoubleArray.From(pool[j]);

                    // add one solution that is 2 pi above
                    if (newSolution[i] + MathConstants.TwoPI < kc.JointList[i].MaxValue)
                    {
                        newSolution[i] += MathConstants.TwoPI;
                        pool.Add(newSolution);
                    }

                    newSolution = DoubleArray.From(pool[j]);

                    // add one solution that is 2 pi below
                    if (newSolution[i] - MathConstants.TwoPI > kc.JointList[i].MinValue)
                    {
                        newSolution[i] -= MathConstants.TwoPI;
                        pool.Add(newSolution);
                    }
                }
            }

            return pool;
        }

        /// <summary>
        /// Some default joint configurations that can be used as good IK seeds.
        /// These joint configurations were collected during our pick-and-place tasks when
        /// the arm was at poses that look natural to the human data collector.
        /// These typical joint configurations do not contain ALL potentially good
        /// joint configurations and are not expected to work with ALL pick-and-place
        /// tasks.  We have them here only for our current pick-and-place tests that
        /// we run every day.
        /// </summary>
        /// <returns>A list of good seed joint configurations</returns>
        public static List<double[]> GetMicoDefaultIKSeeds()
        {
            List<double[]> configList = new List<double[]>();

            // place holder
            configList.Add(new double[] { 1.64, 1.87, 4.59, 3.7, 5.17, 0.104 });

            return configList;
        }
    }
}
